#ifndef SYSTEMATICROBOT_H
#define SYSTEMATICROBOT_H

// Include files.
#include <vector>
#include "WPILib.h"
#include "RobotSubsystem.hpp"
#include "RobotController.hpp"
#include "CompoundController.hpp"

/**
 *  The SystematicRobot class holds all the controllers and subsystems and calls them when necessary.
 *  
 *  SystematicRobot is a cooler version of IterativeRobot.
 *  
 *  To use it, create a subclass and override the Initialize function.
 *  In Initialize, add the subsystems and controllers of your robot.
 *  Be sure to add an autonomous and a teleop controller that will activate and deactivate
 *  the other controllers when necessary.
 */
class SystematicRobot :
	public RobotBase
{
	public:
		static const double sDefaultPeriod;
		
		SystematicRobot();
		virtual ~SystematicRobot();
		virtual void StartCompetition();
		virtual void Initialize();
		const std::set<RobotSubsystem*> &GetSubsystems();
		void AddSubsystem(RobotSubsystem& subsystem);
		void AddSubsystem(RobotSubsystem* subsystem);
		const std::set<RobotController*> &GetControllers();
		void AddController(RobotController& controller);
		void AddEnabledController(RobotController& controller);
		void SetAutonomousController(RobotController &controller);
		void SetTeleopController(RobotController &controller);
		double GetPeriod();
		void SetPeriod(double period);
		double GetLoopsPerSec();
	private:
		enum Mode
		{
			ModeDisabled   = 0x00,
			ModeAutonomous = 0x01,
			ModeTeleop     = 0x02,
		};
		
		Mode m_mode;
		double m_period;
		Timer m_timer;
		std::set<RobotSubsystem*> m_subsystems;
		std::set<RobotController*> m_controllers;
		CompoundController m_controllerEnabled;
		RobotController *m_controllerAutonomous;
		RobotController *m_controllerTeleop;
		
		void Run();
		bool GetIsNextPeriodReady();
		void DelayTask();
		void HandleSubsystemsInitalization();
		void HandleSubsystemsEnabled();
		void HandleSubsystemsDisabled();
		void HandleSubsystemsPeriodic();
		void HandleSubsystemsContinuous();
		void HandleControllersPeriodic();
		void HandleControllersContinuous();
};
	
#endif // #ifndef SYSTEMATICROBOT_H 
